/**
* This file is part of MultiCol-SLAM
*
* Copyright (C) 2015-2016 Steffen Urban <urbste at googlemail.com>
* For more information see <https://github.com/urbste/MultiCol-SLAM>
*
* MultiCol-SLAM is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* MultiCol-SLAM is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with MultiCol-SLAM . If not, see <http://www.gnu.org/licenses/>.
*/

/*
* MultiCol-SLAM is based on ORB-SLAM2 which was also released under GPLv3
* For more information see <https://github.com/raulmur/ORB_SLAM2>
* Ra�l Mur-Artal <raulmur at unizar dot es> (University of Zaragoza)
*/

#include "cViewer.h"
#ifndef _DEBUG
#include <pangolin/pangolin.h>
#endif
#include <mutex>
#include <opencv2/opencv.hpp>
#include <vector>
#include <string>
#include <opencv2/highgui.hpp>
#include <uWS/uWS.h>
#include <json/json.h>

namespace MultiColSLAM
{

	cViewer::cViewer(
		cSystem* pSystem,
		cMultiFramePublisher* pFrameDrawer,
		cMapPublisher* pMapDrawer,
		cTracking *pTracking,
		const std::string &strSettingPath) :
		mpSystem(pSystem), mpFrameDrawer(pFrameDrawer), mpMapDrawer(pMapDrawer), mpTracker(pTracking),
		mbFinishRequested(false), mbFinished(true), mbStopped(false), mbStopRequested(false)
	{
		cv::FileStorage fSettings(strSettingPath, cv::FileStorage::READ);

		float fps = fSettings["Camera.fps"];
		if (fps < 1)
			fps = 25;
		mT = 1e3 / fps;

		mImageWidth = fSettings["Camera.width"];
		mImageHeight = fSettings["Camera.height"];
		if (mImageWidth < 1 || mImageHeight < 1)
		{
			mImageWidth = 640;
			mImageHeight = 480;
		}

		mViewpointX = fSettings["Viewer.ViewpointX"];
		mViewpointY = fSettings["Viewer.ViewpointY"];
		mViewpointZ = fSettings["Viewer.ViewpointZ"];
		mViewpointF = fSettings["Viewer.ViewpointF"];

		// drawNrCams = (int)fSettings["Viewer.DrawNrCams"];
		

		int nrCamsTracker = mpTracker->GetNrCams();
		if (drawNrCams > nrCamsTracker)
			drawNrCams = nrCamsTracker;
		
		drawNrCams = 3;
	}

	static const std::string base64_chars =
             "ABCDEFGHIJKLMNOPQRSTUVWXYZ"
             "abcdefghijklmnopqrstuvwxyz"
             "0123456789+/";

	std::string base64_encode(const unsigned char* data, size_t len) {
		std::string encoded_string;
		int val = 0, valb = -6;
		for (size_t i = 0; i < len; i++) {
			val = (val << 8) + data[i];
			valb += 8;
			while (valb >= 0) {
				encoded_string.push_back(base64_chars[(val >> valb) & 0x3F]);
				valb -= 6;
			}
		}
		if (valb > -6) encoded_string.push_back(base64_chars[((val << 8) >> (valb + 8)) & 0x3F]);
		while (encoded_string.size() % 4) encoded_string.push_back('=');
		return encoded_string;
	}

	// 单个图像编码成Base64
	std::string encodeSingleImageToBase64(const cv::Mat& img) {
		std::vector<uchar> buf;
		cv::imencode(".jpg", img, buf);
		auto *enc_msg = reinterpret_cast<const unsigned char*>(buf.data());
		std::string encoded = base64_encode(enc_msg, buf.size());
		return encoded;
	}

	// 多个图像编码成JSON格式的Base64数组
	std::string encodeImageToBase64(const std::vector<cv::Mat>& imgs) {
		Json::Value jsonArray;  // JSON数组
		for (const auto& img : imgs) {
			std::string encodedImage = encodeSingleImageToBase64(img);
			jsonArray.append(encodedImage);  // 将每个Base64图像添加到JSON数组
		}
		Json::StreamWriterBuilder writer;
		std::string jsonString = Json::writeString(writer, jsonArray);  // 序列化为字符串
		return jsonString;
	}


	void cViewer::Run()
	{
		uWS::Hub h;
		bool isConnected = false;
		uWS::WebSocket<uWS::SERVER> *webSocket = nullptr;

		// 启动WebSocket监听服务
		h.listen(8765);
		std::thread websocketThread([&h]() {
			h.run();
		});
		// h.run();

		// 等待WebSocket线程结束
    	// websocketThread.join();


		// 捕获isConnected和webSocket
		h.onConnection([&isConnected, &webSocket](uWS::WebSocket<uWS::SERVER> *ws, uWS::HttpRequest req) {
			std::cout << "WebSocket连接已建立" << std::endl;
			webSocket = ws;
			isConnected = true;  
		});

		// h.onDisconnection([&isConnected, &webSocket](uWS::WebSocket<uWS::SERVER> *ws, int code, char *message, size_t length) {
		// 	std::cout << "WebSocket连接已断开" << std::endl;
		// 	isConnected = false;  
		// 	webSocket = nullptr;
		// });
		
		mbFinished = false;
#ifndef _DEBUG
		// 创建窗口并绑定
		pangolin::CreateWindowAndBind("MultiCol-SLAM: Map Viewer", 1024, 768);

		// 3D 鼠标处理器需要启用深度测试
		glEnable(GL_DEPTH_TEST);

		// 发出我们可能需要的特定OpenGL
		glEnable(GL_BLEND);
		glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);

		// 创建面板
		pangolin::CreatePanel("menu").SetBounds(0.0, 1.0, 0.0, pangolin::Attach::Pix(175));
		// 菜单变量
		pangolin::Var<bool> menuFollowCamera("menu.Follow Camera", false, true);
		pangolin::Var<bool> menuShowPoints("menu.Show Points", true, true);
		pangolin::Var<bool> menuShowKeyFrames("menu.Show KeyFrames", true, true);
		pangolin::Var<bool> menuShowGraph("menu.Show Graph", true, true);
		pangolin::Var<bool> menuLocalizationMode("menu.Localization Mode", false, true);
		pangolin::Var<bool> menuReset("menu.Reset", false, false);

		// 定义相机渲染对象（用于视图/场景浏览）
		pangolin::OpenGlRenderState s_cam(
			pangolin::ProjectionMatrix(1024, 768, mViewpointF, mViewpointF, 512, 389, 0.1, 1000),
			pangolin::ModelViewLookAt(mViewpointX, mViewpointY, mViewpointZ, 0, 0, 0, 0.0, -1.0, 0.0)
			);

		// 添加命名的OpenGL视图区到窗口并提供3D处理器
		pangolin::View& d_cam = pangolin::CreateDisplay()
			.SetBounds(0.0, 1.0, pangolin::Attach::Pix(175), 1.0, -1024.0f / 768.0f)
			.SetHandler(new pangolin::Handler3D(s_cam));

		// OpenGL矩阵
		pangolin::OpenGlMatrix Twc;
		Twc.SetIdentity();
#endif
		// 为每个相机创建命名窗口
		// for (int c = 0; c < drawNrCams; ++c)
		// 	cv::namedWindow("MultiCol-SLAM: Current Frame: "+to_string(c));

		bool bFollow = true;
		bool bLocalizationMode = false;

		while (1)
		{
#ifndef _DEBUG
			// 清除颜色和深度缓冲区
			glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

			// 获取当前OpenGL的MCSPose
			mpMapDrawer->GetCurrentOpenGLMCSPose(Twc);

			// 根据菜单和状态设置相机视角
			if (menuFollowCamera && bFollow)
			{
				s_cam.Follow(Twc);
			}
			else if (menuFollowCamera && !bFollow)
			{
				s_cam.SetModelViewMatrix(pangolin::ModelViewLookAt(mViewpointX, mViewpointY, mViewpointZ, 0, 0, 0, 0.0, -1.0, 0.0));
				s_cam.Follow(Twc);
				bFollow = true;
			}
			else if (!menuFollowCamera && bFollow)
			{
				bFollow = false;
			}

			// 激活视图区并渲染地图
			d_cam.Activate(s_cam);
			glClearColor(1.0f, 1.0f, 1.0f, 1.0f);
			mpMapDrawer->PublishCurrentCamera(Twc);
			if (menuShowKeyFrames || menuShowGraph)
				mpMapDrawer->PublishMultiKeyFrames(menuShowKeyFrames, menuShowGraph);
			if (menuShowPoints)
				mpMapDrawer->PublishMapPoints();

			// 完成帧的渲染
			pangolin::FinishFrame();
#endif
			std::vector<cv::Mat> imgs;
			mpFrameDrawer->DrawMultiFrame(imgs);
			for (int c = 0; c < drawNrCams; ++c)
			{
				// 显示当前帧
				// cv::imshow("MultiCol-SLAM: Current Frame: " + to_string(c), imgs[c]);
				cv::waitKey(1);

				// 检查连接状态并发送图像
				if (isConnected && webSocket) {  // 检查连接状态
					std::string encodedFrame = encodeImageToBase64(imgs);  // JSON格式的Base64数组
    				webSocket->send(encodedFrame.c_str(), encodedFrame.size(), uWS::OpCode::TEXT);
				}

			}

			cv::waitKey(mT);
#ifndef _DEBUG
			// 重置菜单和视图
			if (menuReset)
			{
				menuShowGraph = true;
				menuShowKeyFrames = true;
				menuShowPoints = true;
				menuLocalizationMode = false;
				if (bLocalizationMode)
					mpSystem->DeactivateLocalizationMode();
				bLocalizationMode = false;
				bFollow = true;
				menuFollowCamera = true;
				mpSystem->Reset();
				menuReset = false;
			}
#endif
			// 检查是否停止
			if (Stop())
				while (isStopped())
					std::this_thread::sleep_for(std::chrono::milliseconds(1000));

			// 检查是否完成
			if (CheckFinish())
				break;

			// 重启WebSocket监听服务
			// h.listen(8765);
			// h.run();
		}

		// 设置完成标志
		SetFinish();

		// 等待WebSocket线程结束
    	websocketThread.join();
#ifndef _DEBUG
		// 绑定上下文并退出
		pangolin::BindToContext("MultiCam SLAM: Map Viewer");
		pangolin::Quit();
#endif
	}

	void cViewer::RequestFinish()
	{
		unique_lock<mutex> lock(mMutexFinish);
		mbFinishRequested = true;
	}

	bool cViewer::CheckFinish()
	{
		unique_lock<mutex> lock(mMutexFinish);
		return mbFinishRequested;
	}

	void cViewer::SetFinish()
	{
		unique_lock<mutex> lock(mMutexFinish);
		mbFinished = true;
	}

	bool cViewer::isFinished()
	{
		unique_lock<mutex> lock(mMutexFinish);
		return mbFinished;
	}

	void cViewer::RequestStop()
	{
		unique_lock<mutex> lock(mMutexStop);
		if (!mbStopped)
			mbStopRequested = true;
	}

	bool cViewer::isStopped()
	{
		unique_lock<mutex> lock(mMutexStop);
		return mbStopped;
	}

	bool cViewer::Stop()
	{
		unique_lock<mutex> lock(mMutexStop);
		unique_lock<mutex> lock2(mMutexFinish);

		if (mbFinishRequested)
			return false;
		else if (mbStopRequested)
		{
			mbStopped = true;
			mbStopRequested = false;
			return true;
		}

		return false;

	}

	void cViewer::Release()
	{
		unique_lock<mutex> lock(mMutexStop);
		mbStopped = false;
	}

}